#include <rclcpp/rclcpp.hpp>
// #include <simple_action_server.hpp>
#include <turtle_bt_ros2/action/bt.hpp>
#include "rclcpp_action/rclcpp_action.hpp"
#include "behaviortree_cpp_v3/action_node.h"

#include <tf2/exceptions.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <geometry_msgs/msg/twist.hpp>
# include <turtle_bt_ros2/srv/spawn.hpp>

#include <chrono>
#include <memory>
#include <string>

using std::placeholders::_1;
using namespace std::chrono_literals;
static int flag2 = 0;


class NpcPatrol : public BT::ActionNodeBase {
public:
    explicit NpcPatrol(const std::string &name, const BT::NodeConfiguration &config):
        BT::ActionNodeBase(name, config) {
        if (flag2 == 0)  {
            node_ = rclcpp::Node::make_shared(name);
            tf_buffer_patrol = std::make_unique<tf2_ros::Buffer>(node_->get_clock());
            transform_listener_patrol = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_patrol);
            flag2 = 1;
        }
        publisher_patrol = node_->create_publisher<geometry_msgs::msg::Twist>("/turtle2/cmd_vel", 10);
    }

    NpcPatrol() = delete;

    ~NpcPatrol() {
        RCLCPP_DEBUG(node_->get_logger(), "Shutting down NpcPatrol BT node");
    }

    static BT::PortsList providedPorts() {
        return { BT::OutputPort < std::string > ("npc patrol") };
    }


    BT::NodeStatus tick() override {
        // if (execute_callback()) {
        //     RCLCPP_INFO(node_->get_logger(), "Action NpcPatrol is successful!");
        //     return BT::NodeStatus::SUCCESS;
        // } else {
        //     RCLCPP_INFO(node_->get_logger(), "Everything is ok.");
        //     return BT::NodeStatus::FAILURE;
        // }

        // tf_buffer_patrol = std::make_unique<tf2_ros::Buffer>(node_->get_clock());
        // transform_listener_patrol = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_patrol);

        RCLCPP_INFO(node_->get_logger(), "Starting action NpcPatrol");

        geometry_msgs::msg::TransformStamped transformStamped;

        geometry_msgs::msg::TransformStamped transformStamped_a;

        rclcpp::WallRate loop_rate(5);
        loop_rate.sleep();

        transformStamped_a = tf_buffer_patrol->lookupTransform("turtle2", "point_a", tf2::TimePointZero);
        geometry_msgs::msg::TransformStamped transformStamped_b;

        transformStamped_b = tf_buffer_patrol->lookupTransform("turtle2", "point_b", tf2::TimePointZero);
        distance_a = sqrt(pow(transformStamped_a.transform.translation.x, 2) + pow(transformStamped_a.transform.translation.y, 2));
        distance_b = sqrt(pow(transformStamped_b.transform.translation.x, 2) + pow(transformStamped_b.transform.translation.y, 2));

        if ((direction == "a") || (direction == "b")) {
            if (distance_a < 0.5) {
                RCLCPP_INFO(node_->get_logger(), "Change direction to b");
                direction = "b";
            } else if (distance_b < 0.5) {
                RCLCPP_INFO(node_->get_logger(), "Change direction to a");
                direction = "a";
            }
        } else {
            RCLCPP_INFO(node_->get_logger(), "Change direction to a");
            direction = "a";
        }

        // if (distance_a < distance_b) {
        //     direction = "a";
        //     if(distance_a < 0.5)
        //     {
        //         direction = "b";
        //     }
        // } else {
        //     direction = "b";
        //     if(distance_b < 0.5)
        //     {
        //         direction = "a";
        //     }
        // }

        // node_->get_parameter("/goal_point", direction);
        if (direction == "a") {
            RCLCPP_INFO(node_->get_logger(), "Nav to a");
            transformStamped = transformStamped_a;
            // node_->undeclare_parameter("/goal_point");
            // node_->declare_parameter("/goal_point", "b");
            // node_->set_parameter("b");
        } else if (direction == "b") {
            RCLCPP_INFO(node_->get_logger(), "Nav to b");
            transformStamped = transformStamped_b;
            // node_->undeclare_parameter("/goal_point");
            // node_->declare_parameter("/goal_point", "a");
            // node_->set_parameter("a");
        }

        geometry_msgs::msg::Twist vel_msg;
        vel_msg.angular.z = 1.0 * atan2(transformStamped.transform.translation.y, transformStamped.transform.translation.x);
        vel_msg.linear.x = 0.8;
        publisher_patrol->publish(vel_msg);

        // node_->undeclare_parameter("/goal_point");

        return BT::NodeStatus::RUNNING;
    }

    void halt() override {
        std::cout << name() << ": Halted." << std::endl;
    }

    // bool execute_callback() {
    //     tf_buffer_patrol = std::make_unique<tf2_ros::Buffer>(node_->get_clock());
    //     transform_listener_patrol = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_patrol);

    //     RCLCPP_INFO(node_->get_logger(), "Starting Condition NpcPatrol");

    //     geometry_msgs::msg::TransformStamped transformStamped;

    //     geometry_msgs::msg::TransformStamped transformStamped_a;

    //     rclcpp::WallRate loop_rate(0.5);
    //     loop_rate.sleep();

    //     transformStamped_a = tf_buffer_patrol->lookupTransform("turtle2", "point_a", tf2::TimePointZero);
    //     geometry_msgs::msg::TransformStamped transformStamped_b;

    //     // rclcpp::WallRate loop_rate(10.0);
    //     // loop_rate.sleep();

    //     transformStamped_b = tf_buffer_patrol->lookupTransform("turtle2", "point_b", tf2::TimePointZero);
    //     distance_a = sqrt(pow(transformStamped_a.transform.translation.x, 2) + pow(transformStamped_a.transform.translation.y, 2));
    //     distance_b = sqrt(pow(transformStamped_b.transform.translation.x, 2) + pow(transformStamped_b.transform.translation.y, 2));

    //     if (node_->get_parameter("/goal_point", direction)) {
    //         if (distance_a < 0.5) {
    //             node_->declare_parameter("/goal_point", "b");
    //             RCLCPP_INFO(node_->get_logger(), "Change direction to b");
    //         } else if (distance_b < 0.5) {
    //             node_->declare_parameter("/goal_point", "a");
    //             RCLCPP_INFO(node_->get_logger(), "Change direction to b");
    //         }
    //     } else {
    //         node_->declare_parameter("/goal_point", "a");
    //     }

    //     node_->get_parameter("/goal_point", direction);
    //     if (direction == "a") {
    //         RCLCPP_INFO(node_->get_logger(), "Nav to a");
    //         transformStamped = transformStamped_a;
    //     } else if (direction == "b") {
    //         RCLCPP_INFO(node_->get_logger(), "Nav to b");
    //         transformStamped = transformStamped_b;
    //     }

    //     geometry_msgs::msg::Twist vel_msg;
    //     vel_msg.angular.z = 1.0 * atan2(transformStamped.transform.translation.y, transformStamped.transform.translation.x);
    //     vel_msg.linear.x = 0.8;
    //     publisher_patrol->publish(vel_msg);
    //     return true;
    // }
private:
    rclcpp::Node::SharedPtr node_;
    std::shared_ptr<tf2_ros::TransformListener> transform_listener_patrol{nullptr};
    std::unique_ptr<tf2_ros::Buffer> tf_buffer_patrol;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_patrol{nullptr};
    double distance_a ;
    double distance_b ;
    std::string direction;
};